clear; close all; clc;

saveArg = 0;
saveFiles = 0;

%% Reading data

xCorrect = 23;
yCorrect = 86.542;
medSize = 110;
threshold = 10^-5;
MadMedTheshold = 10;

gradThreshold = 50;
mark = round(50/70*medSize,0);


% Buoyant non-feeder
data = importdata('Exp_Prev_C1\Data_24-10200-10300\B00001.dat');
data = data.data;
x = (data(:,1) - xCorrect)/1000;
y = (data(:,2) - yCorrect)/1000;
u = data(:,3);
v = data(:,4);
noInd = find(u==0&v==0);
x(noInd) = [];
y(noInd) = [];
u(noInd) = [];
v(noInd) = [];
vector = sqrt(u.^2+v.^2);

dxn = unique(x);
dxn = mean(dxn(2:end)-dxn(1:end-1));
dyn = unique(y);
dyn = mean(dyn(2:end)-dyn(1:end-1));
xlin = linspace(min(x),max(x),medSize);
ylin = linspace(min(y),max(y),medSize);
xn = nan(medSize,medSize);
yn = nan(medSize,medSize);
un = nan(medSize,medSize);
vn = un;
un_mad = un;
vn_mad = un;
for i = 2:medSize-1
    for j = 2:medSize-1
        indCheck = x>xlin(i-1) & x<xlin(i+1) & ...
            y>ylin(j-1) & y<ylin(j+1) & vector>threshold;
        xn(j,i) = median(x(indCheck));
        yn(j,i) = median(y(indCheck));
        un(j,i) = median(u(indCheck));
        vn(j,i) = median(v(indCheck));
        un_mad(j,i) = mad(u(indCheck));
        vn_mad(j,i) = mad(v(indCheck));
    end
end
un(abs(un_mad)./abs(un)>MadMedTheshold) = NaN;
vn(abs(vn_mad)./abs(vn)>MadMedTheshold) = NaN;
vectorn = sqrt(un.^2+vn.^2);
thetan = acos(un./vectorn);





% Buoyant feeder
data = importdata('Exp_Prev_C1\Data_27-11400-11500\B00001.dat');
data = data.data;
x = (data(:,1) - xCorrect)/1000;
y = (data(:,2) - yCorrect)/1000;
u = data(:,3);
v = data(:,4);
noInd = find(u==0&v==0);
x(noInd) = [];
y(noInd) = [];
u(noInd) = [];
v(noInd) = [];
vector = sqrt(u.^2+v.^2);

dxf = unique(x);
dxf = mean(dxf(2:end)-dxf(1:end-1));
dyf = unique(y);
dyf = mean(dyf(2:end)-dyf(1:end-1));
xlin = linspace(min(x),max(x),medSize);
ylin = linspace(min(y),max(y),medSize);
xf = nan(medSize,medSize);
yf = nan(medSize,medSize);
uf = nan(medSize,medSize);
vf = uf;
uf_mad = uf;
vf_mad = uf;
for i = 2:medSize-1
    for j = 2:medSize-1
        indCheck = x>xlin(i-1) & x<xlin(i+1) & ...
            y>ylin(j-1) & y<ylin(j+1) & vector>threshold;
        xf(j,i) = median(x(indCheck));
        yf(j,i) = median(y(indCheck));
        uf(j,i) = median(u(indCheck));
        vf(j,i) = median(v(indCheck));
        uf_mad(j,i) = mad(u(indCheck));
        vf_mad(j,i) = mad(v(indCheck));
    end
end
uf(abs(uf_mad)./abs(uf)>MadMedTheshold) = NaN;
vf(abs(vf_mad)./abs(vf)>MadMedTheshold) = NaN;
vectorf = sqrt(uf.^2+vf.^2);
thetaf = acos(uf./vectorf);


% Removing noisy data
grad_vert_nf = (thetan(3:end,:) - thetan(1:end-2,:))/(dyn*2);
grad_horz_nf = (thetan(:,3:end) - thetan(:,1:end-2))/(dxn*2);
removeInd_vert = abs(grad_vert_nf)>gradThreshold;
removeInd_vert = [zeros(1,size(thetan,2));double(removeInd_vert);...
    zeros(1,size(thetan,2))];
removeInd_horz = abs(grad_horz_nf)>gradThreshold;
removeInd_horz = [zeros(size(thetan,1),1),double(removeInd_horz),...
    zeros(size(thetan,1),1)];
removeInd = removeInd_vert==1 | removeInd_horz==1;
thetan(removeInd) = NaN;
vectorn(removeInd) = NaN;

grad_vert_fd = (thetaf(3:end,:) - thetaf(1:end-2,:))/(dyf*2);
grad_horz_fd = (thetaf(:,3:end) - thetaf(:,1:end-2))/(dxf*2);
removeInd_vert = abs(grad_vert_fd)>gradThreshold;
removeInd_vert = [zeros(1,size(thetaf,2));double(removeInd_vert);...
    zeros(1,size(thetaf,2))];
removeInd_horz = abs(grad_horz_fd)>gradThreshold;
removeInd_horz = [zeros(size(thetaf,1),1),double(removeInd_horz),...
    zeros(size(thetaf,1),1)];
removeInd = removeInd_vert==1 | removeInd_horz==1;
thetaf(removeInd) = NaN;
vectorf(removeInd) = NaN;


xn_disp = xn(mark,:)';
xn_disp = xn_disp/max(abs(xn_disp))/2;
xf_disp = xf(mark,:)';
xf_disp = xf_disp/max(abs(xf_disp))/2;

yn_disp = yn(:,40);
yn_disp = (yn_disp-min(yn_disp))/range(yn_disp);
yf_disp = yf(:,40);
yf_disp = (yf_disp-min(yf_disp))/range(yf_disp);


% figure(10); clf;
% subplot(2,2,1); pcolor(thetan); caxis([0,pi]);
% subplot(2,2,2); pcolor(thetaf); caxis([0,pi]);
% subplot(2,2,3); pcolor(log10(abs(vn_mad./vn))); %caxis([0,pi]);
% subplot(2,2,4); pcolor(log10(abs(vf_mad./vf))); %caxis([0,pi]);
% for i = 1:4
%     subplot(2,2,i);
%     shading flat; colorbar;
% end

clearvars -except 'xn*' 'yn*' 'xf*' 'yf*' 'theta*' 'save*';


%% Filtering data

ysn = (yn-min(yn,[],'all'))/range(yn,'all');
ysf = (yf-min(yf,[],'all'))/range(yf,'all');

stencil = 10;
multp = 1.7;
trn = thetan;
for j = 1:size(thetan,1)
    wind = j-stencil:j+stencil;
    wind(wind<1|wind>size(thetan,1)) = [];
    wind = trn(wind,:);
    wind = wind(~isnan(wind));
    
    filtInd = find(thetan(j,:)>mean(wind)+multp*std(wind) | ...
        thetan(j,:)<mean(wind)-multp*std(wind));
    trn(j,filtInd) = NaN;
end

stencil = 10;
multp = 1.1;
trf = thetaf;
for j = 1:size(thetaf,1)
    wind = j-stencil:j+stencil;
    wind(wind<1|wind>size(thetaf,1)) = [];
    wind = trf(wind,:);
    wind = wind(~isnan(wind));
    
    filtInd = find(thetaf(j,:)>mean(wind)+multp*std(wind) | ...
        thetaf(j,:)<mean(wind)-multp*std(wind));
    trf(j,filtInd) = NaN;
end

cutoff = [0.45 0.85];
trn(ysn<cutoff(1)|ysn>cutoff(2)) = NaN;
trf(ysf<cutoff(1)|ysf>cutoff(2)) = NaN;

figure(10); clf;
set(gcf,'Units','normalized','OuterPosition',[0.1 0.2 0.7 0.6]);
subplot(1,2,1);
    for j = 1:size(thetan,2)
        plot(ysn(:,j),thetan(:,j),'b-'); hold on;
        plot(ysn(:,j),trn(:,j),'r-');
    end
    plot(get(gca,'Xlim'),pi/2*[1,1],'k--');
    plot(cutoff(1)*[1,1],get(gca,'YLim'),'k--');
    plot(cutoff(2)*[1,1],get(gca,'YLim'),'k--');
    ylim([0,pi]);
    set(gca,'YTick',0:pi/6:pi);
    set(gca,'YTickLabel',{0,'\pi/6','\pi/3','\pi/2','2\pi/3',...
        '5\pi/6','\pi'})
    grid on;
    title('Non-feeder','FontWeight','normal');
    xlabel('y*');
    ylabel('\theta (rad)');
subplot(1,2,2);
    for j = 1:size(thetan,2)
        plot(ysf(:,j),thetaf(:,j),'b-'); hold on;
        plot(ysf(:,j),trf(:,j),'r-');
    end
    ylim([0,pi]);
    plot(get(gca,'Xlim'),pi/2*[1,1],'k--');
    plot(cutoff(1)*[1,1],get(gca,'YLim'),'k--');
    plot(cutoff(2)*[1,1],get(gca,'YLim'),'k--');
    set(gca,'YTick',0:pi/6:pi);
    set(gca,'YTickLabel',{0,'\pi/6','\pi/3','\pi/2','2\pi/3',...
        '5\pi/6','\pi'})
    grid on;
    title('Feeder','FontWeight','normal');
    xlabel('y*');

clearvars -except 'xn*' 'yn*' 'xf*' 'yf*' 'theta*' 'save*'...
    'tr*' 'ys*' 'cutoff';


%% Polyfitting

stencil = 3;

dtn = nan(size(trn));
dtf = nan(size(trf));
for j = 1:size(trn,2)
for i = 1:size(trn,1)
    wind = i-stencil:i+stencil;
    wind(wind<1|wind>size(trn,1)) = [];
    checkTheta = trn(wind,j);
    checkY = ysn(wind,j);
    p = polyfit(checkY,checkTheta,1);
    dtn(i,j) = p(1);
    
    checkTheta = trf(wind,j);
    checkY = ysf(wind,j);
    p = polyfit(checkY,checkTheta,1);
    dtf(i,j) = p(1);
end
end

% figure(12); clf;
% subplot(2,2,1);
% pcolor(dtn); shading flat; colorbar;
% subplot(2,2,2);
% pcolor(dtf); shading flat; colorbar;
% subplot(2,2,3);
% for j = 1:size(dtn,2)
%     plot(dtn(:,j),'b.'); hold on;
% end
% subplot(2,2,4);
% for j = 1:size(dtf,2)
%     plot(dtf(:,j),'r.'); hold on;
% end

%% Polyfitting d/d^2

modA = pi/2 + (-0.2:0.005:0.2);
modC = 0:0.1:20;
function modout = mody(a,b,c,y)
    modout = a+b*y.^c;
end

fitY_nf = nan(size(thetan,2),3);
fitY_fd = nan(size(thetaf,2),3);
for dat = 1:2
    if dat == 1
        cury = ysn;
        curt = trn;
        curc = 'b';
    else
        cury = ysf;
        curt = trf;
        curc = 'r';
    end
for j = 1:size(curt,2)
    checkInd = isnan(cury(:,j))==0 & isnan(curt(:,j))==0;
    if sum(checkInd) > 0
        checkTheta = curt(checkInd,j);
        checkY = cury(checkInd,j);
        
        checkY = (checkY-min(checkY))/range(checkY);
        
        p = polyfit(checkY,checkTheta,1);
        if p(1) >= 0
            flag = 1;
        elseif p(1) < 0
            flag = -1;
        end
        % if mean(checkTheta)>=pi/2
        %     flag = 1;
        % else
        %     flag = -1;
        % end
        if numel(checkTheta) > 30
            errorMat = nan(length(modA),length(modC));
            for ma = 1:length(modA)
            for mc = 1:length(modC)
                modout = mody(modA(ma),flag*range(checkTheta),...
                    modC(mc),checkY);
                resid = log10(modout)-log10(checkTheta);
                errorMat(ma,mc) = mean(resid.^2);
            end
            end
            minInd = find(errorMat==min(errorMat,[],'all'));
            [indA,indC] = ind2sub(size(errorMat),minInd);
            if length(indA)>1 || length(indC)>1
                sprintf('boink %d-%d',dat,j)
                indA = indA(1);
                indC = indC(1);
            end
            
            figure(11); clf;
            plot(checkY,checkTheta,'.','color',curc); hold on;
            plot(checkY,mody(modA(indA),flag*range(checkTheta),modC(indC),...
                checkY),'-','Color',curc);
            % ylim([-0.1,1]);
            xl = get(gca,'XLim');
            yl = get(gca,'YLim');
            text(xl(1)+range(xl)*0.05,yl(2)-range(yl)*0.1,...
                sprintf('col=%d, y = %0.2f + %0.1ex^{%0.2f}',...
                j,modA(indA),flag*range(checkTheta),modC(indC)));
            pause(0.1);
            % pause();
            
            if saveFiles == 1
            if dat == 1
                print(sprintf('sav\\nf%03d',j),'-djpeg');
            else

                print(sprintf('sav\\fd%03d',j),'-djpeg');
            end
            end
            
            if dat == 1
                fitY_nf(j,:) = [modA(indA),...
                    modC(indC),errorMat(minInd(1))];
            elseif dat == 2
                fitY_fd(j,:) = [modA(indA),...
                    modC(indC),errorMat(minInd(1))];
            end
        end
    end
end
end

%% Find moving mean
stencil = 2;

polyMeandtheta2Y_nf = nan(size(fitY_nf,1),1);
polyMeanX_nf = nan(size(fitY_nf,1),1);
for j = 1+stencil:size(polyMeanX_nf)-stencil
    polyMeanX_nf(j) = mean(xn_disp(j-stencil:j+stencil),'omitmissing');
    polyMeandtheta2Y_nf(j) = mean(fitY_nf(j-stencil:j+stencil,2),'omitmissing');
end

polyMeandtheta2Y_fd = nan(size(fitY_fd,1),1);
polyMeanX_fd = nan(size(fitY_fd,1),1);
for j = 1+stencil:size(polyMeanX_fd,1)-stencil
    polyMeanX_fd(j) = mean(xf_disp(j-stencil:j+stencil),'omitmissing');
    polyMeandtheta2Y_fd(j) = mean(fitY_fd(j-stencil:j+stencil,2),'omitmissing');
end

clearvars -except save* xn* yn* xf* yf* theta* fitY* poly*...
    ys* tr*;

%% Plotting 1

filtThresh = 1e-4;

figure(1); clf;
set(gcf,'Units','normalized','OuterPosition',[0.1 0.15 0.7 0.7]);
subplot(1,2,1);
pcolor(xn_disp,yn_disp,thetan); hold on;
plot(-0.25*[1,1],[0,1],'k--','linewidth',3);
shading flat; c=colorbar; caxis([0,pi]);
set(c,'Ticks',0:pi/2:pi,'Ticklabels',0:90:180);
set(gca,'FontSize',18);
xlabel('x*'); ylabel('y*');
ylabel(c,'\theta');
xlim([-0.5,0.5]);
tx = text(-0.45,0.94,'a'); set(tx,'FontSize',26);

% filtIndn = fitY_nf(:,3)<filtThresh;
% filtIndf = fitY_fd(:,3)<filtThresh;
subplot(1,2,2);
% plot(xn_disp(filtIndn),fitY_nf(filtIndn,2),'b.','MarkerSize',20); hold on;
% plot(xf_disp(filtIndf),fitY_fd(filtIndf,2),'r.','MarkerSize',20);
% plot(xn_disp,fitY_nf(:,2),'b.','MarkerSize',20); hold on;
% plot(xf_disp,fitY_fd(:,2),'r.','MarkerSize',20);
% plot(polyMeanX_nf(filtIndn),polyMeandtheta2Y_nf(filtIndn),'b-');
% plot(polyMeanX_fd(filtIndf),polyMeandtheta2Y_fd(filtIndf),'r-');
plot(NaN,NaN,'b.','Markersize',20); hold on;
plot(NaN,NaN,'r.','Markersize',20);
for j = 1:size(fitY_nf,1)
    if fitY_nf(j,3) <= filtThresh
        ms = 20;
    elseif fitY_nf(j,3) > filtThresh    
        ms = 20 - 19*(log10(fitY_nf(j,3)) - log10(filtThresh))/...
        (log10(max(fitY_nf(:,3)))-log10(filtThresh));
    else
        ms = 0.0001;
    end
    plot(xn_disp(j),fitY_nf(j,2),'b.','Markersize',ms); hold on;
    plot(xn_disp(j),fitY_nf(j,2),'ko','Markersize',6.9,'LineWidth',1);
    if fitY_fd(j,3) <= filtThresh
        ms = 20;
    elseif fitY_fd(j,3) > filtThresh    
        ms = 20 - 19*(log10(fitY_fd(j,3)) - log10(filtThresh))/...
        (log10(max(fitY_fd(:,3)))-log10(filtThresh));
    else
        ms = 0.0001;
    end
    plot(xf_disp(j),fitY_fd(j,2),'r.','Markersize',ms); hold on;
    plot(xf_disp(j),fitY_fd(j,2),'ko','Markersize',6.9,'LineWidth',1);
end
xl = [-0.4,0.2];
yl = [-1,20];
xlim(xl); ylim(yl);
% plot(get(gca,'XLim'),[0,0],'k--');
plot(get(gca,'XLim'),[1,1],'k--');
plot([0,0],yl,'k--');
set(gca,'YTick',-5:5:20);
grid on;
xlabel('x*'); ylabel('b');
set(gca,'FontSize',18);
leg = legend('Non-feeder','Feeder','fontsize',14);
tx = text(xl(1)+range(xl)*0.05,yl(2)-range(yl)*0.07,'b');
set(tx,'FontSize',26);

pause(0.1);
x1 = 0.10; x2 = 0.66; y1 = 0.21;
wd = 0.29; ht = 0.72;
subplot(1,2,1); set(gca,'Position',[x1 y1 wd ht]);
subplot(1,2,2); set(gca,'Position',[x2 y1 wd ht]);
set(leg,'Position',[0.67 0.67 0.14 0.13]);
% set(leg,'Position',[0.63 0.75 0.32 0.17]);


%% Error plot

figure(2); clf;
plot(xn_disp,fitY_nf(:,3),'b.','markersize',20); hold on;
plot(xf_disp,fitY_fd(:,3),'r.','markersize',20);
set(gca,'YScale','log');
plot(get(gca,'Xlim'),1e-4*[1,1],'k--');
text(-0.47,1.4e-4,'threshold value','fontsize',12);
legend('Non-feeder','Feeder','location','northwest');
xlabel('x*');
ylabel('Error (log_{10}(rad))');
set(gca,'FontSize',12);


%% Saving

if saveArg == 1
    figure(1);
    print(gcf,'gradient','-djpeg','-r300');

    figure(10);
    print(gcf,'SI1_crop','-djpeg','-r300');
    
    figure(2);
    print(gcf,'SI2_error','-djpeg','-r300');

    clearvars -except save* xn* yn* xf* yf* theta* fitY* poly*;
    save('output.mat');
end

